
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_fbwa.c
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
static enum ModeNumber mfbwa_number(mode_base_const_t mode);
static const char *mfbwa_name(mode_base_const_t mode);
static const char *mfbwa_name4(mode_base_const_t mode);
static bool mfbwa_mode_allows_autotuning();
static void mfbwa_update(mode_base_t mode);
/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_fbwa_ops = {
        .run                                = NULL,
        .mode_number                        = mfbwa_number,
        .name                               = mfbwa_name,
        .name4                              = mfbwa_name4,
        .update                             = mfbwa_update,
        .is_vtol_mode                       = NULL,
        .is_vtol_man_throttle               = NULL,
        .is_vtol_man_mode                   = NULL,
        .is_guided_mode                     = NULL,
        .allows_terrain_disable             = NULL,
        .does_automatic_thermal_switch      = NULL,
        .navigate                           = NULL,
        .allows_throttle_nudging            = NULL,
        .does_auto_navigation               = NULL,
        .does_auto_throttle                 = NULL,
        .mode_allows_autotuning             = mfbwa_mode_allows_autotuning,
        .update_target_altitude             = NULL,
        .handle_guided_request              = NULL,
        .is_landing                         = NULL,
        .is_taking_off                      = NULL,
        ._enter                             = NULL,
        ._exit                              = NULL,
        ._pre_arm_checks                    = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mfbwa_ctor(struct ModeFBWA *mode_fbwa)
{
    mode_ctor(&mode_fbwa->mode, &mode_fbwa_ops);
}

// returns a unique number specific to this mode
static enum ModeNumber mfbwa_number(mode_base_const_t mode)
{
    (void)mode;
    return MN_FLY_BY_WIRE_A;
}

// returns full text name
static const char *mfbwa_name(mode_base_const_t mode)
{
    (void)mode;
    return "FLY_BY_WIRE_A";
}

// returns a string for this flightmode, exactly 4 bytes
static const char *mfbwa_name4(mode_base_const_t mode)
{
    (void)mode;
    return "FBWA";
}

static bool mfbwa_mode_allows_autotuning(mode_base_const_t mode)
{
    (void)mode;
    return true;
}

static void mfbwa_update(mode_base_t mode)
{
    // set nav_roll and nav_pitch using sticks
    fms.nav_roll_cd  = rc_norm_input(fms.channel_roll) * fms.roll_limit_cd;
    fms_update_load_factor();
    float pitch_input = rc_norm_input(fms.channel_pitch);
    if (pitch_input > 0) {
        fms.nav_pitch_cd = pitch_input * fms.aparm.pitch_limit_max_cd;
    } else {
        fms.nav_pitch_cd = -(pitch_input * fms.pitch_limit_min_cd);
    }
    fms_adjust_nav_pitch_throttle();
    fms.nav_pitch_cd = math_constrain_int32(fms.nav_pitch_cd, fms.pitch_limit_min_cd, fms.aparm.pitch_limit_max_cd);
    if (fms_fly_inverted()) {
        fms.nav_pitch_cd = -fms.nav_pitch_cd;
    }
    if (fms.failsafe.rc_failsafe && fms.g.fs_action_short == FS_ACTION_SHORT_FBWA) {
        // FBWA failsafe glide
        fms.nav_roll_cd = 0;
        fms.nav_pitch_cd = 0;
        srv_channels_set_output_limit(k_throttle, SRV_MIN);
    }
    rc_channel_t chan = rcs_find_channel_for_option(RC_AUX_FBWA_TAILDRAGGER);
    if (chan != NULL) {
        // check for the user enabling FBWA taildrag takeoff mode
        bool tdrag_mode = rc_get_aux_switch_pos(chan) == RC_AUX_SWITCH_HIGH;
        if (tdrag_mode && !fms.auto_state.fbwa_tdrag_takeoff_mode) {
            if (fms.auto_state.highest_airspeed < fms.g.takeoff_tdrag_speed1) {
                fms.auto_state.fbwa_tdrag_takeoff_mode = true;
                gcs_send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
            }
        }
    }
}
/*------------------------------------test------------------------------------*/


